#ifndef VECTORMATHS_H
#define VECTORMATHS_H

/***************************************************************************/
/* This file contains a number of helper functions and classes used        */
/* to speed up calculations.  It does not form part of the published       */
/* EDEM API. You should copy it to the same directory as the rest of your  */
/* plugin source files.  These functions and methods are not versioned and */
/* may change between releases of EDEM.                                    */
/*                                                                         */
/* All functions and declarations are contained purely within this header  */
/***************************************************************************/

#include <math.h>

namespace NApiEDEM
{

    /***************************************************************************/
    /* Forward declarations of classes in this file                            */
    /***************************************************************************/
    class C3dValue;
    class C3dVector;
    class C3dPoint;
    class C3x3Matrix;

    /***************************************************************************/
    /* Constant definitions of use in calculations                             */
    /***************************************************************************/

    const double PI = 4 * atan(1.0);
    const double DOUBLE_UNDERFLOW = 1e-25;
    const double REALLY_REALLY_ZERO = 1e-40;

    /***************************************************************************/
    /* Helper functions for double comparison                                  */
    /***************************************************************************/

    /**
    * Checks if a double is zero.  As doubles produce small variations
    * due to rounding this method returns true if
    * -DOUBLE_UNDERFLOW < val < DOUBLE_UNDERFLOW
    * @param val The value to check
    * @return bool true if the value is zero (within the tolerance)
    */
    template <class T>
    bool isZero(const T val)
    {
        if(val < DOUBLE_UNDERFLOW && val > (-DOUBLE_UNDERFLOW))
        {
            return true;
        }
        else
        {
            return false;
        }
    }

    /**
    * Checks if a double is zero.  As doubles produce small variations
    * due to rounding this method returns true if
    * -tolerance < val < tolerance
    * The tolerance defaults to REALLY_REALLY_ZERO which is substantially more
    * limiting than the value used by isZero()
    * @param val The value to check
    * @param tolerance The tolerance to check to
    * @return bool true if the value is zero (within the tolerance)
    */
    template <class T>
    inline bool isReallyReallyZero(const T val, const T tolerance = REALLY_REALLY_ZERO)
    {
        if(val <= tolerance && val >= (-tolerance))
        {
            return true;
        }
        else
        {
            return false;
        }
    }

    /**
    * Compares 2 doubles.  As doubles produce small variations due to
    * rounding this method allows the difference to be up to
    * REALLY_REALLY_ZERO (or another user supplied value)
    * @param valA The first value to check
    * @param valB The second value to check
    * @param tolerance The tolerance to check to
    * @return bool true if the values are the same (within the tolerance)
    */
    template <class T>
    inline bool areEqual(const T valA, const T valB, const T tolerance = REALLY_REALLY_ZERO)
    {
        if(!isReallyReallyZero(valB, tolerance))
        {
            // valB is not zero
            return ( fabs(valA/valB - 1.0) < 1e-12 );
        }
        else if(!isReallyReallyZero(valA, tolerance))
        {
            // valA is not zero
            return ( fabs(valB/valA - 1.0) < 1e-12 );
        }
        else
        {
            // Both values are effectivly zero so they are the same
            return true;
        }
    }
    //*************************************************************************
    /**                                                                       
     * Definition of the underlying base class for the classes C3dVector and  
     * C3dPoint.                                                              
     */                                                                       
    //*************************************************************************
    class C3dValue
    {
    public:
        C3dValue() {m_nX = 0; m_nY = 0; m_nZ = 0;}

        C3dValue(const C3dValue &value);

        C3dValue(double dx, double dy, double dz) {m_nX = dx; m_nY = dy; m_nZ = dz;}

        virtual ~C3dValue() {}

        inline C3dValue& operator=(const C3dValue &value);
        inline C3dValue  operator+(const C3dValue &value) const;
        inline C3dValue& operator+=(const C3dValue &value);

        inline C3dValue  operator-(const C3dValue &value) const;
        inline C3dValue  operator-() const;
        inline C3dValue& operator-=(const C3dValue &value);

        inline C3dValue  operator*(const double C3dValue) const;
        inline C3dValue& operator*=(const double multValue);

        inline C3dValue  operator/(const double divValue) const;
        inline C3dValue& operator/=(const double divValue);

        inline bool operator==(const C3dValue& value) const;
        inline bool operator!=(const C3dValue& value) const;

        inline double           x() const       {return m_nX;}
        inline double           y() const       {return m_nY;}
        inline double           z() const       {return m_nZ;}
        inline void             setX(double x)  {m_nX = x;}
        inline void             setY(double y)  {m_nY = y;}
        inline void             setZ(double z)  {m_nZ = z;}

    protected:
        void CheckForUnderflow();
        double m_nX, m_nY, m_nZ;
    };


    /***************************************************************************/
    /* Class definitions for representation of matrices, vectors and points in */
    /* 3D space                                                                */
    /***************************************************************************/

    /**
    * The C3dVector class represents a vector in 3D space.
    * Points and vectors differ in the operations that can be performed on them
    */
    class C3dVector : public C3dValue
    {
    public:
        C3dVector() {m_nX = 0; m_nY = 0; m_nZ = 0;}

        C3dVector(const C3dVector &vector);

        C3dVector(double dx, double dy, double dz) {m_nX = dx; m_nY = dy; m_nZ = dz;}

        virtual ~C3dVector() {}

        inline C3dVector& operator=(const C3dVector &vector);
        inline C3dVector  operator+(const C3dVector &vector) const;
        inline C3dVector& operator+=(const C3dVector &vector);

        inline C3dVector  operator-(const C3dVector &vector) const;
        inline C3dVector  operator-() const;
        inline C3dVector& operator-=(const C3dVector &vector);

        inline C3dVector  operator*(const double multValue) const;
        inline C3dVector  operator*(const C3x3Matrix& matrix) const;
        inline C3dVector& operator*=(const double multValue);
        inline C3dVector& operator*=(const C3x3Matrix& matrix);

        inline C3dVector  operator/(const double divValue) const;
        inline C3dVector& operator/=(const double divValue);

        inline bool operator==(const C3dVector& vector) const;
        inline bool operator!=(const C3dVector& vector) const;

        inline C3dVector  normalise();
        inline C3dVector  getNormal() const;
        inline C3dVector  cross(const C3dVector &vector) const;
        inline double     dot(const C3dVector &vector) const;
        inline double     length() const        {return sqrt(m_nX * m_nX + m_nY * m_nY + m_nZ * m_nZ);}
        inline double     lengthSquared() const {return m_nX * m_nX + m_nY * m_nY + m_nZ * m_nZ;}
    };

    /**
    * The C3dPoint class represents a point in 3D space
    * Points and vectors differ in the operations that can be performed on them
    */
    class C3dPoint : public C3dValue
    {
    public:
        C3dPoint() {m_nX = 0; m_nY = 0; m_nZ = 0; }

        C3dPoint(const C3dPoint &point);

        C3dPoint(double in_x, double in_y, double in_z) {m_nX = in_x; m_nY = in_y; m_nZ = in_z;}

        virtual ~C3dPoint() {}

        inline C3dPoint& operator=(const C3dPoint &point);
        inline C3dPoint  operator+(const C3dVector &vector) const;
        inline C3dPoint& operator+=(const C3dVector &vector);

        inline C3dVector operator-(const C3dPoint &point) const;
        inline C3dPoint  operator-(const C3dVector &vector) const;
        inline C3dPoint& operator-=(const C3dVector &vector);

        inline C3dPoint  operator*( double multValue) const;
        inline C3dPoint& operator*=( double multValue);

        inline C3dPoint  operator/( double divValue) const;
        inline C3dPoint& operator/=( double divValue);

        inline bool operator==(const C3dPoint &point) const;
        inline bool operator!=(const C3dPoint &point) const;

        inline double distance(const C3dPoint &point) const;
    };

    /**
    * The C3x3Matrix class represents a 3x3 matrix.
    * Various operations to access and manipulate the matrix are provided
    */
    class C3x3Matrix
    {
    public:
        C3x3Matrix();

        C3x3Matrix(const C3x3Matrix &matrix);

        C3x3Matrix (double dxx, double dxy, double dxz,
                    double dyx, double dyy, double dyz,
                    double dzx, double dzy, double dzz);

        C3x3Matrix(const C3dVector &vector, double rotAngle);

        virtual ~C3x3Matrix() {}

        inline C3x3Matrix& operator=(const C3x3Matrix &matrix);
        inline C3x3Matrix  operator+(const C3x3Matrix &matrix) const;
        inline C3x3Matrix& operator+=(const C3x3Matrix &matrix);

        inline C3x3Matrix  operator-(const C3x3Matrix &matrix) const;
        inline C3x3Matrix& operator-=(const C3x3Matrix &matrix);

        inline C3x3Matrix  operator*(const double multValue) const;
        inline C3x3Matrix  operator*(const C3x3Matrix &matrix) const;
        inline C3x3Matrix& operator*=(const double multValue);
        inline C3x3Matrix& operator*=(const C3x3Matrix &matrix);

        inline C3x3Matrix  operator/(const double divValue) const;
        inline C3x3Matrix& operator/=(const double divValue);

        inline double det() const;
        inline C3x3Matrix  inv() const;
        inline C3x3Matrix  transpose() const;

        inline double  XX() const        {return m_nXX;}
        inline double  XY() const        {return m_nXY;}
        inline double  XZ() const        {return m_nXZ;}
        inline double  YX() const        {return m_nYX;}
        inline double  YY() const        {return m_nYY;}
        inline double  YZ() const        {return m_nYZ;}
        inline double  ZX() const        {return m_nZX;}
        inline double  ZY() const        {return m_nZY;}
        inline double  ZZ() const        {return m_nZZ;}
        inline void    setXX(double XX) {m_nXX = XX;}
        inline void    setXY(double XY) {m_nXY = XY;}
        inline void    setXZ(double XZ) {m_nXZ = XZ;}
        inline void    setYX(double YX) {m_nYX = YX;}
        inline void    setYY(double YY) {m_nYY = YY;}
        inline void    setYZ(double YZ) {m_nYZ = YZ;}
        inline void    setZX(double ZX) {m_nZX = ZX;}
        inline void    setZY(double ZY) {m_nZY = ZY;}
        inline void    setZZ(double ZZ) {m_nZZ = ZZ;}

    private:
        inline virtual void CheckForUnderflow();
        double m_nXX, m_nXY, m_nXZ, m_nYX, m_nYY, m_nYZ, m_nZX, m_nZY, m_nZZ;
    };


    /***************************************************************************/
    /* Inline Implementations of all C3dValue methods                    */
    /***************************************************************************/

    inline C3dValue::C3dValue(const C3dValue &value)
    {
        m_nX = value.m_nX;
        m_nY = value.m_nY;
        m_nZ = value.m_nZ;

        CheckForUnderflow();
    }

    inline void C3dValue::CheckForUnderflow()
    {
        if(isZero(m_nX)) m_nX = 0;
        if(isZero(m_nY)) m_nY = 0;
        if(isZero(m_nZ)) m_nZ = 0;
    }

    inline C3dValue& C3dValue::operator=(const C3dValue &value)
    {
        if(this!=&value)
        {
            m_nX = value.x();
            m_nY = value.y();
            m_nZ = value.z();

            CheckForUnderflow();
        }

        return *this;
    }

    inline C3dValue C3dValue::operator+(const C3dValue &value) const
    {
        C3dValue retVal;

        retVal.setX(m_nX + value.x());
        retVal.setY(m_nY + value.y());
        retVal.setZ(m_nZ + value.z());

        return retVal;

    }

    inline C3dValue& C3dValue::operator+=(const C3dValue &value)
    {
        m_nX += value.x();
        m_nY += value.y();
        m_nZ += value.z();

        CheckForUnderflow();

        return *this;
    }

    inline C3dValue C3dValue::operator-(const C3dValue &value) const
    {
        return  C3dValue(m_nX - value.x(), m_nY - value.y(), m_nZ - value.z());
    }

    inline C3dValue C3dValue::operator-() const
    {
        return  C3dValue(-m_nX, -m_nY, -m_nZ);
    }

    inline C3dValue& C3dValue::operator-=(const C3dValue &value)
    {
        m_nX -= value.x();
        m_nY -= value.y();
        m_nZ -= value.z();

        CheckForUnderflow();

        return *this;
    }

    inline C3dValue C3dValue::operator*(const double multValue) const
    {
        C3dPoint retVal;

        retVal.setX (m_nX * multValue);
        retVal.setY (m_nY * multValue);
        retVal.setZ (m_nZ * multValue);

        return retVal;
    }

    inline C3dValue& C3dValue::operator*=(const double multValue)
    {
        m_nX *= multValue;
        m_nY *= multValue;
        m_nZ *= multValue;

        CheckForUnderflow();

        return *this;
    }

    inline C3dValue C3dValue::operator/(const double divValue) const
    {
        C3dPoint retVal;

        if(!isZero(divValue))
        {
            retVal.setX (m_nX / divValue);
            retVal.setY (m_nY / divValue);
            retVal.setZ (m_nZ / divValue);
        }
        else
        {
            retVal.setX (9E30);
            retVal.setY (9E30);
            retVal.setZ (9E30);
        }

        return retVal;
    }

    inline C3dValue& C3dValue::operator/=(const double divValue)
    {
        if(!isZero(divValue))
        {
            m_nX /= divValue;
            m_nY /= divValue;
            m_nZ /= divValue;
        }
        else
        {
            m_nX = 9E30;
            m_nY = 9E30;
            m_nZ = 9E30;
        }

        CheckForUnderflow();

        return *this;
    }

    inline bool C3dValue::operator==(const C3dValue &value)  const
    {
        if(isZero(m_nX - value.m_nX) && isZero(m_nY - value.m_nY) && isZero(m_nZ - value.m_nZ))
        {
            return true;
        }

        return false;
    }

    inline bool C3dValue::operator!=(const C3dValue &value)  const
    {
        return !(*this == value);
    }

    /***************************************************************************/
    /* Inline Implementations of all C3dVector methods                   */
    /***************************************************************************/

    inline C3dVector::C3dVector(const C3dVector &vector)
    :C3dValue()
    {
        m_nX = vector.m_nX;
        m_nY = vector.m_nY;
        m_nZ = vector.m_nZ;

        CheckForUnderflow();
    }

    inline C3dVector& C3dVector::operator=(const C3dVector &vector)
    {
        if(this!=&vector)
        {
            m_nX = vector.x();
            m_nY = vector.y();
            m_nZ = vector.z();

            CheckForUnderflow();
        }

        return *this;
    }

    inline C3dVector C3dVector::operator+(const C3dVector &vector) const
    {
        C3dVector retVal;

        retVal.setX( vector.m_nX + m_nX);
        retVal.setY( vector.m_nY + m_nY);
        retVal.setZ( vector.m_nZ + m_nZ);

        return retVal;
    }

    inline C3dVector& C3dVector::operator+=(const C3dVector &vector)
    {
        m_nX += vector.m_nX;
        m_nY += vector.m_nY;
        m_nZ += vector.m_nZ;

        CheckForUnderflow();

        return *this;
    }

    inline C3dVector C3dVector::operator-(const C3dVector &vector) const
    {
        C3dVector retVal;

        retVal.setX( m_nX - vector.m_nX);
        retVal.setY( m_nY - vector.m_nY);
        retVal.setZ( m_nZ - vector.m_nZ);

        return retVal;
    }

    inline C3dVector C3dVector::operator-() const
    {
        return  C3dVector(-m_nX, -m_nY, -m_nZ);
    }

    inline C3dVector& C3dVector::operator-=(const C3dVector &vector)
    {
        m_nX -= vector.m_nX;
        m_nY -= vector.m_nY;
        m_nZ -= vector.m_nZ;

        CheckForUnderflow();

        return *this;
    }

    inline C3dVector C3dVector::operator*(const double multValue) const
    {
        C3dVector retVal;

        retVal.setX( m_nX * multValue);
        retVal.setY( m_nY* multValue);
        retVal.setZ( m_nZ * multValue);

        return retVal;
    }

    inline C3dVector C3dVector::operator*(const C3x3Matrix& matrix) const
    {
        C3dVector retVect;

        retVect.setX( m_nX * matrix.XX() + m_nY * matrix.XY() + m_nZ * matrix.XZ());
        retVect.setY( m_nX * matrix.YX() + m_nY * matrix.YY() + m_nZ * matrix.YZ());
        retVect.setZ( m_nX * matrix.ZX() + m_nY * matrix.ZY() + m_nZ * matrix.ZZ());

        return retVect;
    }

    inline C3dVector& C3dVector::operator*=(const double multValue)
    {
        m_nX *= multValue;
        m_nY *= multValue;
        m_nZ *= multValue;

        CheckForUnderflow();

        return *this;
    }

    inline C3dVector& C3dVector::operator*=(const C3x3Matrix& matrix)
    {
        C3dVector tmp(*this); 
        m_nX = tmp.m_nX * matrix.XX() + tmp.m_nY * matrix.XY() + tmp.m_nZ * matrix.XZ();
        m_nY = tmp.m_nX * matrix.YX() + tmp.m_nY * matrix.YY() + tmp.m_nZ * matrix.YZ();
        m_nZ = tmp.m_nX * matrix.ZX() + tmp.m_nY * matrix.ZY() + tmp.m_nZ * matrix.ZZ();

        CheckForUnderflow();

        return *this;
    }

    inline C3dVector C3dVector::operator/(const double divValue) const
    {
        C3dVector retVal;

        if(!isZero(divValue))
        {
            retVal.setX( m_nX / divValue);
            retVal.setY( m_nY  / divValue);
            retVal.setZ( m_nZ  / divValue);
        }
        else
        {
            retVal.setX( 9E30);
            retVal.setY( 9E30);
            retVal.setZ( 9E30);
        }

        return retVal;
    }

    inline C3dVector& C3dVector::operator/=(const double divValue)
    {
        if(!isZero(divValue))
        {
            m_nX /= divValue;
            m_nY /= divValue;
            m_nZ /= divValue;
        }
        else
        {
            m_nX = 9E30;
            m_nY = 9E30;
            m_nZ = 9E30;
        }

        CheckForUnderflow();

        return *this;
    }

    inline bool C3dVector::operator==(const C3dVector& vector) const
    {
        return (areEqual(m_nX, vector.m_nX)) && (areEqual(m_nY, vector.m_nY)) && (areEqual(m_nZ, vector.m_nZ));
    }


    inline bool C3dVector::operator!=(const C3dVector& vector) const
    {
        return !(*this == vector);
    }

    inline C3dVector C3dVector::normalise()
    {
        double nLength = length();

        if(!isZero(nLength)) //only normalise if length isn't zero.
        {
            m_nX /= nLength;
            m_nY /= nLength;
            m_nZ /= nLength;
        }

        CheckForUnderflow();

        return *this;
    }

    inline C3dVector C3dVector::getNormal() const
    {
        C3dVector retVal(m_nX, m_nY, m_nZ);
        double nLength = retVal.length();

        if(!isZero(nLength)) //only normalise if length isn't zero.
        {
            retVal.m_nX /= nLength;
            retVal.m_nY /= nLength;
            retVal.m_nZ /= nLength;
        }

        retVal.CheckForUnderflow();

        return retVal;
    }

    inline C3dVector C3dVector::cross(const C3dVector &vector) const
    {
        C3dVector retVect;

        retVect.setX( m_nY * vector.m_nZ - m_nZ * vector.m_nY );
        retVect.setY( m_nZ * vector.m_nX - m_nX * vector.m_nZ );
        retVect.setZ( m_nX * vector.m_nY - m_nY * vector.m_nX );

        return retVect;
    }

    inline double C3dVector::dot(const C3dVector &vector) const
    {
        return m_nX * vector.m_nX + m_nY * vector.m_nY + m_nZ * vector.m_nZ;
    }

    /***************************************************************************/
    /* Inline Implementations of all C3dPoint methods                    */
    /***************************************************************************/

    inline C3dPoint::C3dPoint(const C3dPoint &point)
        :C3dValue()
    {
        m_nX = point.m_nX;
        m_nY = point.m_nY;
        m_nZ = point.m_nZ;

        CheckForUnderflow();
    }

    inline C3dPoint& C3dPoint::operator=(const C3dPoint &point)
    {
        if(this!=&point)
        {
            m_nX = point.x();
            m_nY = point.y();
            m_nZ = point.z();

            CheckForUnderflow();
        }

        return *this;
    }

    inline C3dPoint C3dPoint::operator+(const C3dVector &vector) const
    {
        C3dPoint retVal;

        retVal.setX(m_nX + vector.x());
        retVal.setY(m_nY + vector.y());
        retVal.setZ(m_nZ + vector.z());

        return retVal;
    }

    inline C3dPoint& C3dPoint::operator+=(const C3dVector &vector)
    {
        m_nX += vector.x();
        m_nY += vector.y();
        m_nZ += vector.z();

        CheckForUnderflow();

        return *this;
    }

    inline C3dVector C3dPoint::operator-(const C3dPoint &point) const
    {
        C3dVector vector(m_nX - point.m_nX, m_nY - point.m_nY, m_nZ - point.m_nZ);
        return vector;
    }

    inline C3dPoint C3dPoint::operator-(const C3dVector &vector) const
    {
        C3dPoint point(m_nX - vector.x(), m_nY - vector.y(), m_nZ - vector.z());
        return point;
    }

    inline C3dPoint& C3dPoint::operator-=(const C3dVector &vector)
    {
        m_nX -= vector.x();
        m_nY -= vector.y();
        m_nZ -= vector.z();

        CheckForUnderflow();

        return *this;
    }

    inline C3dPoint C3dPoint::operator*(double multValue) const
    {
        C3dPoint retVal;

        retVal.setX (m_nX * multValue);
        retVal.setY (m_nY * multValue);
        retVal.setZ (m_nZ * multValue);

        return retVal;
    }

    inline C3dPoint& C3dPoint::operator*=(double multValue)
    {
        m_nX *= multValue;
        m_nY *= multValue;
        m_nZ *= multValue;

        CheckForUnderflow();

        return *this;
    }

    inline C3dPoint C3dPoint::operator/(double divValue) const
    {
        C3dPoint retVal;

        if(!isZero(divValue))
        {
            retVal.setX (m_nX / divValue);
            retVal.setY (m_nY / divValue);
            retVal.setZ (m_nZ / divValue);
        }
        else
        {
            retVal.setX (9E30);
            retVal.setY (9E30);
            retVal.setZ (9E30);
        }

        return retVal;
    }

    inline C3dPoint& C3dPoint::operator/=(double divValue)
    {
        if(!isZero(divValue))
        {
            m_nX /= divValue;
            m_nY /= divValue;
            m_nZ /= divValue;
        }
        else
        {
            m_nX = 9E30;
            m_nY = 9E30;
            m_nZ = 9E30;
        }

        CheckForUnderflow();

        return *this;
    }

    inline bool C3dPoint::operator==(const C3dPoint &point) const
    {
        if(isZero(m_nX - point.m_nX) && isZero(m_nY - point.m_nY) && isZero(m_nZ - point.m_nZ))
        {
            return true;
        }

        return false;
    }

    inline bool C3dPoint::operator!=(const C3dPoint &point) const
    {
        return !(*this == point);
    }

    inline double C3dPoint::distance(const C3dPoint &point) const
    {
        return sqrt((m_nX-point.x())*(m_nX-point.x()) + (m_nY-point.y())*(m_nY-point.y()) + (m_nZ-point.z())*(m_nZ-point.z()));
    }

    /***************************************************************************/
    /* Inline Implementations of all C3x3Matrix methods                  */
    /***************************************************************************/

    inline C3x3Matrix::C3x3Matrix()
        :m_nXX(1),
        m_nXY(0),
        m_nXZ(0),
        m_nYX(0),
        m_nYY(1),
        m_nYZ(0),
        m_nZX(0),
        m_nZY(0),
        m_nZZ(1)
    {
        ; // deliberately left empty
    }

    inline C3x3Matrix::C3x3Matrix(const C3x3Matrix &matrix)
    {
        m_nXX = matrix.m_nXX;
        m_nXY = matrix.m_nXY;
        m_nXZ = matrix.m_nXZ;
        m_nYX = matrix.m_nYX;
        m_nYY = matrix.m_nYY;
        m_nYZ = matrix.m_nYZ;
        m_nZX = matrix.m_nZX;
        m_nZY = matrix.m_nZY;
        m_nZZ = matrix.m_nZZ;

        CheckForUnderflow();
    }

    inline C3x3Matrix::C3x3Matrix(double dxx, double dxy, double dxz,
                                  double dyx, double dyy, double dyz,
                                  double dzx, double dzy, double dzz)
    :m_nXX(dxx),
    m_nXY(dxy),
    m_nXZ(dxz),
    m_nYX(dyx),
    m_nYY(dyy),
    m_nYZ(dyz),
    m_nZX(dzx),
    m_nZY(dzy),
    m_nZZ(dzz)
    {
        ;
    }

    inline C3x3Matrix::C3x3Matrix(const C3dVector &vector, double rotAngle)
    {
        double c = cos(rotAngle);
        double s = sin(rotAngle);

        m_nXX = 1 + ( 1 - c ) * ( vector.x() * vector.x() - 1 );
        m_nXY = -vector.z() * s + ( 1 - c ) * vector.x() * vector.y();
        m_nXZ = vector.y() * s + ( 1 - c ) * vector.x() * vector.z();
        m_nYX = vector.z() * s + ( 1 - c ) * vector.x() * vector.y();
        m_nYY = 1 + ( 1 - c ) * ( vector.y() * vector.y() - 1 );
        m_nYZ = -vector.x() * s + ( 1 - c ) * vector.y() * vector.z();
        m_nZX = -vector.y() * s + ( 1 - c ) * vector.x() * vector.z();
        m_nZY = vector.x() * s + ( 1 - c ) * vector.y() * vector.z();
        m_nZZ = 1 + ( 1 - c ) * ( vector.z() * vector.z() - 1 );

        CheckForUnderflow();
    }

    inline C3x3Matrix& C3x3Matrix::operator=(const C3x3Matrix &matrix)
    {
        if(this != &matrix)
        {
            m_nXX = matrix.XX();
            m_nXY = matrix.XY();
            m_nXZ = matrix.XZ();
            m_nYX = matrix.YX();
            m_nYY = matrix.YY();
            m_nYZ = matrix.YZ();
            m_nZX = matrix.ZX();
            m_nZY = matrix.ZY();
            m_nZZ = matrix.ZZ();
        }

        return *this;
    }

    inline C3x3Matrix C3x3Matrix::operator+(const C3x3Matrix &matrix) const
    {
        C3x3Matrix retVal;

        retVal.setXX( matrix.m_nXX + m_nXX);
        retVal.setXY( matrix.m_nXY + m_nXY);
        retVal.setXZ( matrix.m_nXZ + m_nXZ);
        retVal.setYX( matrix.m_nYX + m_nYX);
        retVal.setYY( matrix.m_nYY + m_nYY);
        retVal.setYZ( matrix.m_nYZ + m_nYZ);
        retVal.setZX( matrix.m_nZX + m_nZX);
        retVal.setZY( matrix.m_nZY + m_nZY);
        retVal.setZZ( matrix.m_nZZ + m_nZZ);

        return retVal;
    }

    inline C3x3Matrix& C3x3Matrix::operator+=(const C3x3Matrix &matrix)
    {
        m_nXX += matrix.m_nXX;
        m_nXY += matrix.m_nXY;
        m_nXZ += matrix.m_nXZ;
        m_nYX += matrix.m_nYX;
        m_nYY += matrix.m_nYY;
        m_nYZ += matrix.m_nYZ;
        m_nZX += matrix.m_nZX;
        m_nZY += matrix.m_nZY;
        m_nZZ += matrix.m_nZZ;

        CheckForUnderflow();

        return *this;
    }

    inline C3x3Matrix C3x3Matrix::operator-(const C3x3Matrix &matrix) const
    {
        C3x3Matrix retVal;

        retVal.setXX( m_nXX - matrix.m_nXX);
        retVal.setXY( m_nXY - matrix.m_nXY);
        retVal.setXZ( m_nXZ - matrix.m_nXZ);
        retVal.setYX( m_nYX - matrix.m_nYX);
        retVal.setYY( m_nYY - matrix.m_nYY);
        retVal.setYZ( m_nYZ - matrix.m_nYZ);
        retVal.setZX( m_nZX - matrix.m_nZX);
        retVal.setZY( m_nZY - matrix.m_nZY);
        retVal.setZZ( m_nZZ - matrix.m_nZZ);

        return retVal;
    }

    inline C3x3Matrix& C3x3Matrix::operator-=(const C3x3Matrix &matrix)
    {
        m_nXX -= matrix.m_nXX;
        m_nXY -= matrix.m_nXY;
        m_nXZ -= matrix.m_nXZ;
        m_nYX -= matrix.m_nYX;
        m_nYY -= matrix.m_nYY;
        m_nYZ -= matrix.m_nYZ;
        m_nZX -= matrix.m_nZX;
        m_nZY -= matrix.m_nZY;
        m_nZZ -= matrix.m_nZZ;

        CheckForUnderflow();

        return *this;
    }

    inline C3x3Matrix C3x3Matrix::operator*(const double multValue) const
    {
        C3x3Matrix retVal;

        retVal.setXX( m_nXX  * multValue);
        retVal.setXY( m_nXY  * multValue);
        retVal.setXZ( m_nXZ  * multValue);
        retVal.setYX( m_nYX  * multValue);
        retVal.setYY( m_nYY  * multValue);
        retVal.setYZ( m_nYZ  * multValue);
        retVal.setZX( m_nZX  * multValue);
        retVal.setZY( m_nZY  * multValue);
        retVal.setZZ( m_nZZ  * multValue);

        return retVal;
    }

    inline C3x3Matrix C3x3Matrix::operator*(const C3x3Matrix &matrix) const
    {
        C3x3Matrix retVal;

        retVal.setXX( m_nXX * matrix.m_nXX + m_nXY * matrix.m_nYX + m_nXZ * matrix.m_nZX);
        retVal.setXY( m_nXX * matrix.m_nXY + m_nXY * matrix.m_nYY + m_nXZ * matrix.m_nZY);
        retVal.setXZ( m_nXX * matrix.m_nXZ + m_nXY * matrix.m_nYZ + m_nXZ * matrix.m_nZZ);
        retVal.setYX( m_nYX * matrix.m_nXX + m_nYY * matrix.m_nYX + m_nYZ * matrix.m_nZX);
        retVal.setYY( m_nYX * matrix.m_nXY + m_nYY * matrix.m_nYY + m_nYZ * matrix.m_nZY);
        retVal.setYZ( m_nYX * matrix.m_nXZ + m_nYY * matrix.m_nYZ + m_nYZ * matrix.m_nZZ);
        retVal.setZX( m_nZX * matrix.m_nXX + m_nZY * matrix.m_nYX + m_nZZ * matrix.m_nZX);
        retVal.setZY( m_nZX * matrix.m_nXY + m_nZY * matrix.m_nYY + m_nZZ * matrix.m_nZY);
        retVal.setZZ( m_nZX * matrix.m_nXZ + m_nZY * matrix.m_nYZ + m_nZZ * matrix.m_nZZ);

        return retVal;
    }

    inline C3x3Matrix& C3x3Matrix::operator*=(const double multValue)
    {
        m_nXX  *= multValue;
        m_nXY  *= multValue;
        m_nXZ  *= multValue;
        m_nYX  *= multValue;
        m_nYY  *= multValue;
        m_nYZ  *= multValue;
        m_nZX  *= multValue;
        m_nZY  *= multValue;
        m_nZZ  *= multValue;

        CheckForUnderflow();

        return *this;
    }

    inline C3x3Matrix& C3x3Matrix::operator*=(const C3x3Matrix &matrix)
    {
        C3x3Matrix tmp(*this);
        m_nXX = tmp.m_nXX * matrix.m_nXX + tmp.m_nXY * matrix.m_nYX + tmp.m_nXZ * matrix.m_nZX;
        m_nXY = tmp.m_nXX * matrix.m_nXY + tmp.m_nXY * matrix.m_nYY + tmp.m_nXZ * matrix.m_nZY;
        m_nXZ = tmp.m_nXX * matrix.m_nXZ + tmp.m_nXY * matrix.m_nYZ + tmp.m_nXZ * matrix.m_nZZ;
        m_nYX = tmp.m_nYX * matrix.m_nXX + tmp.m_nYY * matrix.m_nYX + tmp.m_nYZ * matrix.m_nZX;
        m_nYY = tmp.m_nYX * matrix.m_nXY + tmp.m_nYY * matrix.m_nYY + tmp.m_nYZ * matrix.m_nZY;
        m_nYZ = tmp.m_nYX * matrix.m_nXZ + tmp.m_nYY * matrix.m_nYZ + tmp.m_nYZ * matrix.m_nZZ;
        m_nZX = tmp.m_nZX * matrix.m_nXX + tmp.m_nZY * matrix.m_nYX + tmp.m_nZZ * matrix.m_nZX;
        m_nZY = tmp.m_nZX * matrix.m_nXY + tmp.m_nZY * matrix.m_nYY + tmp.m_nZZ * matrix.m_nZY;
        m_nZZ = tmp.m_nZX * matrix.m_nXZ + tmp.m_nZY * matrix.m_nYZ + tmp.m_nZZ * matrix.m_nZZ;

        CheckForUnderflow();

        return *this;
    }

    inline C3x3Matrix C3x3Matrix::operator/(const double divValue) const
    {
        C3x3Matrix retVal;

        if(!isZero(divValue))
        {
            retVal.setXX( m_nXX  / divValue);
            retVal.setXY( m_nXY  / divValue);
            retVal.setXZ( m_nXZ  / divValue);
            retVal.setYX( m_nYX  / divValue);
            retVal.setYY( m_nYY  / divValue);
            retVal.setYZ( m_nYZ  / divValue);
            retVal.setZX( m_nZX  / divValue);
            retVal.setZY( m_nZY  / divValue);
            retVal.setZZ( m_nZZ  / divValue);
        }
        else
        {
            retVal.setXX( 9E30);
            retVal.setXY( 9E30);
            retVal.setXZ( 9E30);
            retVal.setYX( 9E30);
            retVal.setYY( 9E30);
            retVal.setYZ( 9E30);
            retVal.setZX( 9E30);
            retVal.setZY( 9E30);
            retVal.setZZ( 9E30);
        }

        return retVal;
    }

    inline C3x3Matrix& C3x3Matrix::operator/=(const double divValue)
    {
        if(!isZero(divValue))
        {
            m_nXX  /= divValue;
            m_nXY  /= divValue;
            m_nXZ  /= divValue;
            m_nYX  /= divValue;
            m_nYY  /= divValue;
            m_nYZ  /= divValue;
            m_nZX  /= divValue;
            m_nZY  /= divValue;
            m_nZZ  /= divValue;

        }
        else
        {
            m_nXX  = 9E30;
            m_nXY  = 9E30;
            m_nXZ  = 9E30;
            m_nYX  = 9E30;
            m_nYY  = 9E30;
            m_nYZ  = 9E30;
            m_nZX  = 9E30;
            m_nZY  = 9E30;
            m_nZZ  = 9E30;
        }

        CheckForUnderflow();

        return *this;
    }

    inline double C3x3Matrix::det() const
    {
        // Our matrix is organised as:
        //
        // XX XY XZ
        // YX YY YZ
        // ZX ZY ZZ
        //
        // The formula for The determinant of a 3x3 matrix of this form is:
        //
        // XX*YY*ZZ + XY*YZ*ZX + XZ*YX*ZY - XX*YZ*ZY - XY*YX*ZZ - XZ*YY*ZX
        //
        // from:
        //
        // http://en.wikipedia.org/wiki/Determinant#3-by-3_matrices

        return m_nXX * m_nYY * m_nZZ +
               m_nXY * m_nYZ * m_nZX +
               m_nXZ * m_nYX * m_nZY -
               m_nXX * m_nYZ * m_nZY -
               m_nXY * m_nYX * m_nZZ -
               m_nXZ * m_nYY * m_nZX;
    }

    inline C3x3Matrix C3x3Matrix::inv() const
    {
        C3x3Matrix retVal;

        // Our matrix is organised as:
        //
        // XX XY XZ
        // YX YY YZ
        // ZX ZY ZZ
        //
        // The formula for the inverse of a matrix using Cramer's rule is:
        //
        //     Inverse(A) = 1/Determinant(A) * Adjugate(A)
        //
        // Where
        //
        //     Adjugate(A) is The Transpose of The Cofactor matrix of A
        //
        // from:
        //
        // http://en.wikipedia.org/wiki/Cramer's_rule#Finding_inverse_matrix

        // First calculate the determinant
        double ndet = det();

        // If we have a determinant of 0 we can't get the inverse
        // Set all values of the return matrix to be invalid values
        if (isZero(ndet))
        {
            retVal.setXX(9E30);
            retVal.setXY(9E30);
            retVal.setXZ(9E30);
            retVal.setYX(9E30);
            retVal.setYY(9E30);
            retVal.setYZ(9E30);
            retVal.setZX(9E30);
            retVal.setZY(9E30);
            retVal.setZZ(9E30);
            return retVal;
        }

        // Calculate the inverse
        retVal.setXX( (m_nYY*m_nZZ - m_nYZ*m_nZY) / ndet );
        retVal.setXY( (m_nXZ*m_nZY - m_nXY*m_nZZ) / ndet );
        retVal.setXZ( (m_nXY*m_nYZ - m_nXZ*m_nYY) / ndet );
        retVal.setYX( (m_nYZ*m_nZX - m_nYX*m_nZZ) / ndet );
        retVal.setYY( (m_nXX*m_nZZ - m_nXZ*m_nZX) / ndet );
        retVal.setYZ( (m_nXZ*m_nYX - m_nXX*m_nYZ) / ndet );
        retVal.setZX( (m_nYX*m_nZY - m_nYY*m_nZX) / ndet );
        retVal.setZY( (m_nXY*m_nZX - m_nXX*m_nZY) / ndet );
        retVal.setZZ( (m_nXX*m_nYY - m_nXY*m_nYX) / ndet );

        return retVal;
    }

    inline C3x3Matrix C3x3Matrix::transpose() const
    {
        C3x3Matrix retVal;

        // Our matrix is organised as:
        // XX XY XZ
        // YX YY YZ
        // ZX ZY ZZ

        //It needs to be returned as:
        // XX YX ZX
        // XY YY ZY
        // XZ YZ ZZ

        retVal.setXX( m_nXX );
        retVal.setXY( m_nYX );
        retVal.setXZ( m_nZX );
        retVal.setYX( m_nXY );
        retVal.setYY( m_nYY );
        retVal.setYZ( m_nZY );
        retVal.setZX( m_nXZ );
        retVal.setZY( m_nYZ );
        retVal.setZZ( m_nZZ );

        return retVal;
    }

    inline void C3x3Matrix::CheckForUnderflow()
    {
        if(isZero(m_nXX)) m_nXX = 0;
        if(isZero(m_nXY)) m_nXY = 0;
        if(isZero(m_nXZ)) m_nXZ = 0;
        if(isZero(m_nYX)) m_nYX = 0;
        if(isZero(m_nYY)) m_nYY = 0;
        if(isZero(m_nYZ)) m_nYZ = 0;
        if(isZero(m_nZX)) m_nZX = 0;
        if(isZero(m_nZY)) m_nZY = 0;
        if(isZero(m_nZZ)) m_nZZ = 0;
    }
}

#endif //VECTORMATHS_H
